#include "Seek.h"
#include "../Obstacle.h"
#include "../CApp.h"

#include <iostream>
#include <vector>

using namespace std;

Seek::Seek(vector<Obstacle *>* _obstacle){
//  Seek(){
  maxAcceleration = 0.001;
  obstacle = _obstacle;
}

Seek::Seek(float maxAcceleration){
  this->maxAcceleration = maxAcceleration;
}

Steering* Seek::getSteering(){
  Steering* ret = new Steering();

  // Caminos en el grafo del mapa
  Node *start = NULL, *end = NULL;
  vector<Node *>camino;
  if(theCharacter){
    start = theApp.findNearestNode(character->position);
  }
  if(theTarget){
    end = theApp.findNearestNode(target->position);
  }
  theCharacter->caminoObjetivo.clear();
  if(start && end){
    if(start->getId() != end->getId()){
      camino = theApp.getGraph()->dijkstra(start, end);
      theCharacter->caminoObjetivo = camino;

      // Actualizamos las posiciones
      target->position[0] = camino[1]->getPosition()[0];
      target->position[1] = camino[1]->getPosition()[1];
    }
  }
  // FIN calculo de caminos

  restarVectores(target->position, character->position,ret->linear);
  normalize(ret->linear);
  multVectorScalar(ret->linear,maxAcceleration);
  return ret;
  // return Obs_Sep(ret,obstacle,character);
}
